#include "vehicle_linear2d2s.h"
#include <iostream>
using namespace auto_ros::control;

int main()
{
	//	VehicleLinear2d2s(double mass, double cf, double cr, double lf, double lr, double iz,
	//double max_lateral_acc = 9, double max_tire_f_angle = 0.5);
	VehicleLinear2d2s test_c(1500, 48000, 48000, 1.015, 1.895, 1536.7, 7, 0.5);
	std::cout << "init state_matrix is:" << std::endl
			  << test_c.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << test_c.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << test_c.distur_matrix() << std::endl;

	test_c.speed_update_matrix(1.0); //shoud be same as init
	std::cout << "1m/s state_matrix is:" << std::endl
			  << test_c.state_matrix() << std::endl;
	std::cout << "1m/s control matrix is:" << std::endl
			  << test_c.control_matrix() << std::endl;
	std::cout << "1m/s distur matrix is:" << std::endl
			  << test_c.distur_matrix() << std::endl;
	test_c.speed_update_matrix(2.0);
	std::cout << "2m/s state_matrix is:" << std::endl
			  << test_c.state_matrix() << std::endl;
	std::cout << "2m/s control matrix is:" << std::endl
			  << test_c.control_matrix() << std::endl;
	std::cout << "2m/s distur matrix is:" << std::endl
			  << test_c.distur_matrix() << std::endl;

	//test_dis eular
	//std::shared_ptr<DiscreteLinearSys> ContinuousLinearSys::discretization(double delta_time,
	//																   std::string method, double exact_torrence)
	std::shared_ptr<DiscreteLinearSys> test_dis = test_c.discretization(0.01); //just test,1.0s is too large
	std::cout << "2m/s dis 1s state_matrix is:" << std::endl
			  << test_dis->state_matrix() << std::endl;
	std::cout << "2m/s dis 1s control matrix is:" << std::endl
			  << test_dis->control_matrix() << std::endl;
	std::cout << "2m/s dis 1s distur matrix is:" << std::endl
			  << test_dis->distur_matrix() << std::endl;
	//test tustin
	test_c.discrete(test_dis, 0.01, "tustin"); //just test,1.0s is too large
	std::cout << "tustin way 2m/s dis 1s state_matrix is:" << std::endl
			  << test_dis->state_matrix() << std::endl;
	std::cout << "tustin way  2m/s dis 1s control matrix is:" << std::endl
			  << test_dis->control_matrix() << std::endl;
	std::cout << "tustin way  2m/s dis 1s distur matrix is:" << std::endl
			  << test_dis->distur_matrix() << std::endl;
	test_c.discrete(test_dis, 0.5);
	std::cout << "tustin way  2m/s dis 0.5s state_matrix is:" << std::endl
			  << test_dis->state_matrix() << std::endl;
	std::cout << "tustin way  2m/s dis 0.5s control matrix is:" << std::endl
			  << test_dis->control_matrix() << std::endl;
	std::cout << "tustin way  2m/s dis 0.5s distur matrix is:" << std::endl
			  << test_dis->distur_matrix() << std::endl;

	test_c.discrete(test_dis, 0.01);

	/*ContinuousLinearSys::integral(
				const uint segment, const double start_time, const double end_time,
				const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd xf) *
				*/
	unsigned int state_dim = test_c.state_dim();
	unsigned int control_dim = test_c.control_dim();
	unsigned int distur_dim = test_c.distur_dim();
	Eigen::VectorXd x0(state_dim);
	x0 << 0.0, 0, 0, 0;
	Eigen::VectorXd u_fb(control_dim);
	u_fb << 0;
	Eigen::MatrixXd distur_ref(distur_dim, 1);
	distur_ref << 1.0;
	Eigen::VectorXd xf(state_dim);
	double speed = 1;
	test_c.speed_update_matrix(speed);
	int max_iter = 100;
	for (int i = 0; i < 100; i++)
	{
		test_c.integral(1, 0, 0.02, x0, u_fb, distur_ref, xf, "tustin");
		std::cout << ""
				  << " x is:" << xf.transpose() << std::endl;
		x0 = xf;
	}
}